/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/
#ifndef __MMM_ModelNode_H_
#define __MMM_ModelNode_H_

#include "../MMMCore.h"
#include "../MMMImportExport.h"
#include "../XMLTools.h"

#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>

namespace MMM
{
//! Describes different units.
enum UnitInfo
{
	eNone,		//<! Not specified
	eMM,		//<! Millimeter
	eM,			//<! Meter
	eRadian,	//<! Radian
	eDegree,	//<! Degree
	eKG,		//<! Kilogram
	eG			//<! Gram
};

//! Currently only fixed (no joint) and revolute joints are supported.
enum JointType
{
	eFixed,		//<! No == fixed joint
    eRevolute,	//<! Revolute joint
    ePrismatic  //<! Prismatic joint
};

/*!
	\brief Stores information about joint properties.
*/
class MMM_IMPORT_EXPORT JointInfo
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	JointInfo()
	{
		jointType=eFixed;
        limitLo=limitHi=offset=initValue=0.0f;
		maxVelocity=maxAcceleration=maxTorque=0.0f;
		limitUnits=eNone;
		axis.setZero();
	}

	//! Limits information
	float limitLo, limitHi;

    //! initalvalue
    float initValue;

    //! Units of limits
	UnitInfo limitUnits;

	float maxVelocity;			//! given in m/s
	float maxAcceleration;		//! given in m/s^2
	float maxTorque;			//! given in Nm

	//! The joint type
	JointType jointType;

    //! Local axis (only used for revolute & prismatic joints)
	Eigen::Vector3f axis;

	//! joint value offset
	float offset;

	//! move these joints when this joint is actuated (name,factor map)
	std::map<std::string,float> propagateJointValues;

    //! Creates an XML string that describes this object.
	std::string toXML( )
	{
		// nothing to do for fixed joints
		if (jointType==eFixed)
			return std::string();

		std::stringstream ss;
		ss << "\t\t<Joint type=";
		switch (jointType)
		{
		case eRevolute:
			ss << "'revolute' > " << endl;
            ss << "\t\t\t<axis x='" << axis[0] << "' y='" << axis[1] << "' z='" << axis[2] << "'/>" << endl;
			break;
        case ePrismatic:
            ss << "'prismatic' > " << endl;
            ss << "\t\t\t<translationdirection x='" << axis[0] << "' y='" << axis[1] << "' z='" << axis[2] << "'/>" << endl;
            break;
		default:
			ss << "'fixed' > " << endl;
            ss << "\t\t\t<axis x='" << axis[0] << "' y='" << axis[1] << "' z='" << axis[2] << "'/>" << endl;
			break;
		}
		ss << "\t\t\t<limits lo='" << limitLo << "' hi='" << limitHi << "' units='radian'/>" << endl;
		ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << endl;
		ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << endl;
		ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << endl;

		std::map< std::string, float >::iterator propIt = propagateJointValues.begin();
		while (propIt!=propagateJointValues.end())
		{
			ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << endl;
			propIt++;
		}

		ss << "\t\t</Joint>" << endl;

		return ss.str();
	}
};

typedef boost::shared_ptr<JointInfo> JointInfoPtr;

/*!
	\brief Information on dynamic properties of the segment.
*/
class MMM_IMPORT_EXPORT SegmentInfo
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	SegmentInfo()
	{
		mass=0;
        com.setZero();
        inertia.setZero();
        scaling = 1.0f;
	}

	//! center of mass in local coordinates (meter)
	Eigen::Vector3f com;

	//! The mass (kg)
	float mass;

	//! Inertia matrix
	Eigen::Matrix3f inertia;

    //! Mass scaling
    float scaling;

	//! Collisions with other segments can be ignored
	std::vector< std::string > ignoreCollisions;

	//! Visualization type (e.g. "Inventor")
	std::string visuType;
	//! Visualization file (optional)
	std::string visuFile;

    //! Creates an XML string that describes this object.
	std::string toXML()
	{
		int tabs = 2;
		std::string ta;
		std::stringstream ss;
		for (int i=0;i<tabs;i++)
			ta += "\t";
		std::string ta2 = ta;
		ta2 += "\t\t";
		ss << ta << "<Physics>\n";
		ss << ta << "\t<Mass unit='kg' value='" << mass << "'/>\n";
		ss << ta << "\t<CoM location=";
		ss << "'Custom' x='" << com(0) << "' y='" << com(1) << "' z='" << com(2) << "'/>\n";
		ss << ta << "\t<InertiaMatrix>\n";
		ss << XML::toXML(inertia,ta2,true);
		ss << ta << "\t</InertiaMatrix>\n";
		for (size_t i=0;i<ignoreCollisions.size();i++)
			ss << ta << "\t<IgnoreCollisions name='" << ignoreCollisions[i] << "'/>\n";
		ss << ta << "</Physics>\n";
		if (!visuType.empty() && !visuFile.empty())
		{
			ss << ta << "<Visualization>" << endl;
			ss << ta << "\t<File type='" << visuType << "'>" << visuFile << "</File>" << endl;
			ss << ta << "\t<UseAsCollisionModel/>" << endl;
			ss << ta << "</Visualization>" << endl;
		}
		return ss.str();
	}
};

typedef boost::shared_ptr<SegmentInfo> SegmentInfoPtr;


/*!
\brief Stores information about marker properties.
*/
class MMM_IMPORT_EXPORT MarkerInfo
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	MarkerInfo()
	{
		localTransform.setIdentity();
	}
	
	std::string name;
	Eigen::Matrix4f localTransform;

	boost::shared_ptr<MarkerInfo> clone()
	{
		boost::shared_ptr<MarkerInfo> res(new MarkerInfo());
		res->name = name;
		res->localTransform = localTransform;
		return res;
	}

	//! Creates an XML string that describes this object.
	std::string toXML()
	{
		std::string ta2 = "\t\t\t\t";
		std::stringstream ss;
        // ss << "\t\t<Sensor type='position' name='" << name << "'/>" << endl;
        ss << "\t\t<Sensor type='position' name='" << name << "'>" << endl;
		ss << "\t\t\t<Transform>\n";
		ss << XML::toXML(localTransform, ta2, false);
		ss << "\t\t\t</Transform>\n";		
		ss << "\t\t</Sensor>" << endl;

		return ss.str();
	}
};

typedef boost::shared_ptr<MarkerInfo> MarkerInfoPtr;


class ModelNode;
typedef boost::shared_ptr<ModelNode> ModelNodePtr;

/*!
	\brief The ModelNode holds kinematic and dynamic information of a segment and/or joint information.
	The structure is as follows:
	-> parent
		-> local Transform
		-> joint Transform
		-> segment
		-> markers
		-> children
			->...
*/
class MMM_IMPORT_EXPORT ModelNode
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	ModelNode(){
		localTransformation.setIdentity();
        scaling = 1.0f;
	};
	
	//! The transformation from parent node (translation part in meter)
	Eigen::Matrix4f localTransformation;

	//! A joint can be specified (optional)
	JointInfo joint;

	//! Segment/Link/Limb information (optional)
	SegmentInfo segment;

	//! All children as string vector
	std::vector< std::string > children;

	//! The markers
	std::vector< MarkerInfoPtr > markers;

	//! Checks if a marker with given name is present in the modelnode
	bool hasMarker(const std::string &markerName) const
	{
		std::vector<MarkerInfoPtr>::const_iterator it = markers.begin();

		while (it != markers.end())
		{
			if ((*it)->name == markerName)
				return true;
			it++;
		}
		return false;
	}

	//! The unique name of this node
	std::string name;

    float scaling;

	std::string toXML()
	{
		std::stringstream ss;
		ss << "\t<RobotNode name='" << name << "'>" << endl;
		if (!localTransformation.isIdentity())
		{
			ss << "\t\t<Transform>" << endl;
			ss << XML::toXML(localTransformation, "\t\t\t");
			ss << "\t\t</Transform>" << endl;
		}

		ss << joint.toXML();
		ss << segment.toXML();
		for (size_t i = 0; i < markers.size(); i++)
		{
			ss << markers[i]->toXML();
		}

		for (size_t i = 0; i < children.size(); i++)
		{
			ss << "\t\t<Child name='" << children[i] << "'/>" << endl;
		}
		ss << "\t</RobotNode>" << endl << endl;
		return ss.str();
	}

	ModelNodePtr clone()
	{
		ModelNodePtr res(new ModelNode());
		res->localTransformation = localTransformation;
		res->name = name;
		res->joint = joint;
		res->segment = segment;
		res->children = children;
		res->scaling = scaling;
		for (size_t i = 0; i < markers.size(); i++)
			res->markers.push_back(markers[i]->clone());
		return res;
	}
};

}

#endif 
